#include "config.h"
#include "odrive_diff.h"
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <dynamic_reconfigure/server.h>
#include <string>
#include <iostream>
#include <fstream>
#include <math.h>

#define PI 3.141592653
using namespace odrive_can_ros;

odrive_diff::odrive_diff()
{ 
  hardware_interface::JointStateHandle left_wheel_state_handle(
    "left_wheel", &joints[0].pos.data, &joints[0].vel.data, &joints[0].eff.data);

  hardware_interface::JointStateHandle right_wheel_state_handle(
    "right_wheel", &joints[1].pos.data, &joints[1].vel.data, &joints[1].eff.data);

  joint_state_interface.registerHandle(left_wheel_state_handle);
  joint_state_interface.registerHandle(right_wheel_state_handle);
  registerInterface(&joint_state_interface);

  hardware_interface::JointHandle left_wheel_vel_handle(
    joint_state_interface.getHandle("left_wheel"), &joints[0].cmd.data);

  hardware_interface::JointHandle right_wheel_vel_handle(
    joint_state_interface.getHandle("right_wheel"), &joints[1].cmd.data);

  velocity_joint_interface.registerHandle(left_wheel_vel_handle);
  velocity_joint_interface.registerHandle(right_wheel_vel_handle);
  registerInterface(&velocity_joint_interface);

  // These publishers are only for debugging purposes
  state_pub         = nh.advertise<std_msgs::Int8>("odrive/state", 3);  // 发布者 odrive状态  
  vbus_pub          = nh.advertise<std_msgs::Float64>("odrive/vbus", 3); // 发布者 odrive电压
  //sub
  sub               = nh.subscribe("joy", 1000, &odrive_diff::joyCallback,this); //订阅 手柄话题
  odom_encoder_pub  = nh.advertise<nav_msgs::Odometry>("odom", 50); // Odometry 

  // FIXME! Read parameters from ROS
  wheel_radius     = WHEEL_RADIUS; //车轮直径
  wheel_separation = WHEEL_SEPARATION; //车轮轮距

  // State of the driver
  prv_state = AXIS_STATE_IDLE;
  driver_state = AXIS_STATE_IDLE;
  prv_btn_state = 0;

  /****** ODRIVE CAN SETUP *******/

  try{ 
    //添加电机轴0；电机轴1 向can master node 中
    if ( !( master.add_axis(OD_AXIS0_CANID, "odrive_axis_0") && master.add_axis(OD_AXIS1_CANID, "oodrive_axis_1") ) )
    {
      fprintf(stderr, "Failed to create one or more axis. Aborting.\n");
      throw "Device not found!";
    }
    // Create Interface to SocketCAN 
    const std::string can_device = OD_CAN;// const std::string can_device = "can0";
    can::ThreadedSocketCANInterfaceSharedPtr driver = std::make_shared<can::ThreadedSocketCANInterface>();
    if (!driver->init(can_device, 0, can::NoSettings::create()))
    {
      fprintf(stderr, "Failed to initialize can_device at %s\n", can_device.c_str());
      throw "Failed to initialize can_device!";
    }
    can::StateListenerConstSharedPtr state_listener = driver->createStateListener(
        [&driver](const can::State& s) {
          std::string err;
          driver->translateError(s.internal_error, err);
          fprintf(stderr, "CAN Device error: %s, asio: %s.\n",err.c_str(), s.error_code.message().c_str());
        }
    );
    
    master.init(driver);// Pass the SocketCAN handle to master
  }catch(const char* msg) {
    std::cout << msg << std::endl;
  }
  
  val8.data = 0;
  state_pub.publish(val8); // 初始化odrive时发布odrive的状态
}

odrive_diff::~odrive_diff()
{
  // Set velocity 关闭设置速度为0 
  fval = 0.0;
  master.set_input_vel(master.axis("odrive_axis_0"), fval);
  master.set_input_vel(master.axis("odrive_axis_1"), fval);
}

void odrive_diff::read()
{
  /****** ODRIVE CAN Collect data *******/
  /* 
  当前驱动器的状态，无关ros control
  */
  // 读取 电压
  master.get_vbus_voltage(master.axis("odrive_axis_0"));
  vbus.data = master.axis("odrive_axis_0").vbus_voltage;
  vbus_pub.publish(vbus); //发布电压

  // 读取 axis0是否故障
  master.get_motor_error(master.axis("odrive_axis_0"));
  error0 = master.axis("odrive_axis_0").axis_error;
  // 读取 axis1是否故障
  master.get_motor_error(master.axis("odrive_axis_1"));
  error1 = master.axis("odrive_axis_1").axis_error;
  // 读取 axis0当前状态 axis1当前状态
  // readOdriveData(endpoint, odrive_json, std::string("axis0.current_state"), u8val);
  state0 = master.axis("odrive_axis_0").axis_state;
  state1 = master.axis("odrive_axis_0").axis_state;
  if(error0 || error1)
  {
    ROS_ERROR("[ODRIVE] Axis error has ocurred, shutting down..");
    ros::shutdown();
  }

  /****** ODRIVE CAN Collect data *******/
  /* 
  当前驱动器的状态，ros control相关
  */
  master.get_encoder_estimates(master.axis("odrive_axis_0"));
  master.get_encoder_estimates(master.axis("odrive_axis_1"));
  // 读取 速度估计 位移估计
  vel0 = master.axis("odrive_axis_0").vel_enc_estimate;
  vel1 = master.axis("odrive_axis_1").vel_enc_estimate;
  pos0 = master.axis("odrive_axis_0").pos_enc_estimate;
  pos1 = master.axis("odrive_axis_1").pos_enc_estimate;

  joints[0].vel.data = DIRECTION_CORRECTION * (vel0/encoder_CPR) * 2*PI*wheel_radius;
  joints[1].vel.data = DIRECTION_CORRECTION * (vel1/encoder_CPR) * 2*PI*wheel_radius;
  joints[0].pos.data = DIRECTION_CORRECTION * (pos0/encoder_CPR) * 2*PI*wheel_radius;
  joints[1].pos.data = DIRECTION_CORRECTION * (pos1/encoder_CPR) * 2*PI*wheel_radius;

  // printf("Result speed (rad/s): %f %f\n",joints[0].vel.data,joints[1].vel.data);

  // left_vel_pub.publish(vbus);
  // right_vel_pub.publish(joints[1].vel);
  // left_pos_pub.publish(joints[0].pos);
  // right_pos_pub.publish(joints[1].pos);

  // ODOMETRY // 
  // ticks since last encoder read
  double deltaTickr = pos1 - m_tickr;
  double deltaTickl = pos0 - m_tickl;

  // ticks to distance conversion 
  m_Dr =  2*PI*wheel_radius*(deltaTickr/encoder_CPR);
  m_Dl = -2*PI*wheel_radius*(deltaTickl/encoder_CPR);

  // // Distance from wheels to distance from center line conversion 
  m_Dc = (m_Dr+m_Dl)/2;

  // Differential drive robot kinematics
  // Postition estimation 
  double x_p   = m_x   + m_Dc*cos(m_phi);
  double y_p   = m_y   + m_Dc*sin(m_phi);
  double phi_p = m_phi + (m_Dr-m_Dl)/wheel_separation;

  // Velocity estimation
  double xd_p  =  wheel_radius/2*(-vel0+vel1)*cos(m_phi);
  double yd_p  =  wheel_radius/2*(-vel0+vel1)*sin(m_phi);
  double phid_p = (wheel_radius/wheel_separation)*(-vel1+vel0);

    // Publish odometry data 
  current_time = ros::Time::now();

  //since all odometry is 6DOF we'll need a quaternion created from yaw
  geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(phi_p);

  //first, we'll publish the transform over tf
  geometry_msgs::TransformStamped odom_trans;
  odom_trans.header.stamp = current_time;
  odom_trans.header.frame_id = "odom";
  odom_trans.child_frame_id = "base_footprint";

  odom_trans.transform.translation.x = x_p;
  odom_trans.transform.translation.y = y_p;
  odom_trans.transform.translation.z = 0.0;
  odom_trans.transform.rotation = odom_quat;

  //send the transform
  odom_broadcaster.sendTransform(odom_trans);

  //next, we'll publish the odometry message over ROS
  nav_msgs::Odometry odom; // Create odom object 
  odom.header.stamp = current_time; 
  odom.header.frame_id = "odom";  // We will publish this as odom_encoder

  //set the position
  odom.pose.pose.position.x = x_p;
  odom.pose.pose.position.y = y_p;
  odom.pose.pose.position.z = 0.0;
  odom.pose.pose.orientation = odom_quat;

  //set the velocity
  odom.child_frame_id = "base_footprint"; 
  odom.twist.twist.linear.x = xd_p;
  odom.twist.twist.linear.y = yd_p;
  odom.twist.twist.angular.z = phid_p;

  //publish the message
  odom_encoder_pub.publish(odom);

  // Update dynamic variables
  m_x = x_p;
  m_y = y_p;
  m_phi = phi_p;
  m_tickr = pos1;
  m_tickl = pos0;

}

void odrive_diff::write()
{

  // Inform interested parties about the commands we've got
  // left_cmd_pub.publish(joints[0].cmd);
  // right_cmd_pub.publish(joints[1].cmd);

  // Convert rad/s to m/s
  double left_speed = -1 *DIRECTION_CORRECTION * (joints[0].cmd.data/(2*PI)) * encoder_CPR;
  double right_speed =    DIRECTION_CORRECTION * (joints[1].cmd.data/(2*PI)) *  encoder_CPR;
  //
  if(driver_state == AXIS_STATE_IDLE){
    // Set velocity (Save guard velcity zero always)
    fval = 0;
    master.set_input_vel(master.axis("odrive_axis_0"), fval);
    master.set_input_vel(master.axis("odrive_axis_1"), fval);

    if(prv_state == AXIS_STATE_CLOSED_LOOP_CONTROL){
      // Set idle only once when comming from closed loop control
      // 关闭闭环控制状态
      master.set_axis_requested_state(master.axis("odrive_axis_0"), AxisState::AXIS_STATE_IDLE);
      master.set_axis_requested_state(master.axis("odrive_axis_1"), AxisState::AXIS_STATE_IDLE);
      prv_state = AXIS_STATE_IDLE;
      val8.data = 0;
      state_pub.publish(val8);
	}
  }else if(driver_state == AXIS_STATE_CLOSED_LOOP_CONTROL){
    if(prv_state == AXIS_STATE_IDLE)
    {
      // Arm motors only once when comming back from idle state
      master.set_axis_requested_state(master.axis("odrive_axis_0"), AxisState::AXIS_STATE_CLOSED_LOOP_CONTROL);
      master.set_axis_requested_state(master.axis("odrive_axis_1"), AxisState::AXIS_STATE_CLOSED_LOOP_CONTROL);
      prv_state = AXIS_STATE_CLOSED_LOOP_CONTROL;
      val8.data = 1;
      state_pub.publish(val8);
    }
    
    // Set velocity
    fval = (float)left_speed;
    master.set_input_vel(master.axis("odrive_axis_0"), fval);
    fval = (float)right_speed;
    master.set_input_vel(master.axis("odrive_axis_1"), fval);
  }
}

void odrive_diff::updateWD(void){
  // update watchdog
  // execOdriveFunc(endpoint, odrive_json, "axis0.watchdog_feed");
  // execOdriveFunc(endpoint, odrive_json, "axis1.watchdog_feed");
}

void odrive_diff::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
{   
    // If it was low and now its high
    if(prv_btn_state == 0 && joy->buttons[2] == 1){
        if(driver_state == AXIS_STATE_IDLE)
        {
            printf("[ODRIVE] Motors armed.\n");
            driver_state = AXIS_STATE_CLOSED_LOOP_CONTROL;
        }else
        {
            printf("[ODRIVE] Motors disarmed.\n");
            driver_state = AXIS_STATE_IDLE;
        }
    }
    prv_btn_state = joy->buttons[2];
}
